/* Copyright 2014-2021 The MathWorks, Inc. */

#ifndef _SLROS_GENERIC_PUBSUB_H_
#define _SLROS_GENERIC_PUBSUB_H_

#include <iostream>
#include <ros/ros.h>
#include <ros/callback_queue.h>

extern ros::NodeHandle*
    SLROSNodePtr; ///< The global node handle that is used by all ROS entities in the model

/**
 * Class for subscribing to ROS messages in C++.
 *
 * This class is used by code generated from the Simulink ROS
 * subscriber blocks and is templatized by the ROS message type and
 * Simulink bus type.
 */
template <class MsgType, class BusType>
class SimulinkSubscriber {
  public:
    void subscriberCallback(const boost::shared_ptr<MsgType const>&);
    void createSubscriber(std::string const& topic, uint32_t queueSize);
    bool getLatestMessage(BusType* busPtr); // returns true iff message is new

  private:
    boost::shared_ptr<ros::CallbackQueue> _customCallbackQueuePtr;
    ros::Subscriber _subscriber;
    bool _newMessageReceived;
    boost::shared_ptr<MsgType const> _lastMsgPtr;
    boost::mutex _mtx;
};

/**
 * Callback that is triggered when a new message is received
 *
 * This function will store the message as latest message.
 * @param msgPtr The received message
 */
template <class MsgType, class BusType>
void SimulinkSubscriber<MsgType, BusType>::subscriberCallback(
    const boost::shared_ptr<MsgType const>& msgPtr) {
    boost::lock_guard<boost::mutex> lockMsg(_mtx);
    _lastMsgPtr = msgPtr; // copy the shared_ptr
    _newMessageReceived = true;
}

/**
 * Create a C++ subscriber object
 *
 * @param topic The topic name to subscribe to
 * @param queueSize The length of incoming message queue
 */
template <class MsgType, class BusType>
void SimulinkSubscriber<MsgType, BusType>::createSubscriber(std::string const& topic,
                                                            uint32_t queueSize) {
    _customCallbackQueuePtr.reset(new ros::CallbackQueue());

    ros::SubscribeOptions opts;
    opts.init<MsgType>(
        topic, queueSize,
        boost::bind(&SimulinkSubscriber<MsgType, BusType>::subscriberCallback, this, _1));
    opts.callback_queue = _customCallbackQueuePtr.get();

    _subscriber = SLROSNodePtr->subscribe(opts);
}

/**
 * Get the latest received message
 *
 * @param busPtr Simulink bus structure that should be populated with message contents
 * @return =TRUE, then a new message has been received and *busPtr holds the newly-received message.
 * =FALSE when a new message has not been received and *busPtr is unchanged.
 */
template <class MsgType, class BusType>
bool SimulinkSubscriber<MsgType, BusType>::getLatestMessage(BusType* busPtr) {
    _customCallbackQueuePtr->callOne();

    if (_newMessageReceived) {
        boost::lock_guard<boost::mutex> lockMsg(_mtx);
        convertToBus(busPtr, _lastMsgPtr.get());
        _newMessageReceived = false;
        return true; // message is new
    } else {
        return false; // message is not new
    }
}


/**
 * Class for publishing ROS messages in C++.
 *
 * This class is used by code generated from the Simulink ROS
 * publisher blocks and is templatized by the ROS message type and
 * Simulink bus type.
 */
template <class MsgType, class BusType>
class SimulinkPublisher {

  public:
    void createPublisher(std::string const& topic, uint32_t queueSize);
    void publish(BusType* busPtr);

  private:
    ros::Publisher _publisher; ///< The ROS publisher object
    MsgType _msg; ///< A prototype of the message to publish (will be filled based on Simulink bus
                  ///< structure)
};


/**
 * Create a publisher to a topic
 *
 * @param topic The name of the topic to advertise
 * @param queueSize The length of outgoing publishing message queue
 */
template <class MsgType, class BusType>
void SimulinkPublisher<MsgType, BusType>::createPublisher(std::string const& topic,
                                                          uint32_t queueSize) {
    _publisher = SLROSNodePtr->advertise<MsgType>(topic, queueSize);
}

/**
 * Publish a message
 *
 * @param busPtr Pointer to the bus structure for the outgoing message
 */
template <class MsgType, class BusType>
void SimulinkPublisher<MsgType, BusType>::publish(BusType* busPtr) {
    convertFromBus(&_msg, busPtr);
    _publisher.publish(_msg);
}

#ifdef _SL_ROS_CONTROL_PLUGIN_
#include <realtime_tools/realtime_publisher.h>

/**
 * Class for publishing realtime ROS messages in C++.
 *
 * This class is used by code ros_control controller package
 * generated from the Simulink ROS publisher blocks and is templatized by
 * the ROS message type and Simulink bus type.
 */
template <class MsgType, class BusType>
class SimulinkRTPublisher {
  public:
    void createPublisher(std::string const& topic, uint32_t queueSize);
    void publish(BusType* busPtr);

  private:
    realtime_tools::RealtimePublisher<MsgType> _publisher; /// Realtime publisher object
};

/**
 * Initialize the realtime publisher
 *
 * @param topic The name of the topic to advertise
 * @param queueSize The length of outgoing publishing message queue
 */
template <class MsgType, class BusType>
void SimulinkRTPublisher<MsgType, BusType>::createPublisher(std::string const& topic,
                                                            uint32_t queueSize) {
    _publisher.init(*SLROSNodePtr, topic, queueSize);
}

/**
 * Convert from bus and publish the message
 *
 * @param busPtr Pointer to the bus structure for the outgoing message
 */
template <class MsgType, class BusType>
void SimulinkRTPublisher<MsgType, BusType>::publish(BusType* busPtr) {
    if (_publisher.trylock()) {
        convertFromBus(&_publisher.msg_, busPtr);
        _publisher.unlockAndPublish();
    }
}
#endif


#endif
